\section{Sensor Fusion and State Estimation}

One of the most challenging technical aspects of the project is the need to estimate the state of the system. This is because of the difficulty of directly observing the angles and angular rates of the system when considering the project's cost constraints.

In order to maintain a pointing vector at a celestial body, we need only consider the three attitude degrees of freedom. This is because variations in absolution position (latitude, longitude and altitude) make only infinitesimal differences to the pointing vector when observing objects at such great instances.

The balloon system is assumed to be in equilibrium, meaning there exists a 1G gravity vector available to the system as a reference. This is further assumed to be perfectly radially aligned with the Earth. This allows measurements of pitch and roll to be taken. The Earth's magnetic field is also available as a basis to measure yaw.

\subsection{Attitude Sensor Selection}

In general, the rotation of a platform can be measured by a number of means. The attitude can be measured directly, with reference to external phenomena such as gravity or celestial bodies, or by tracking the inertial effect of rotation. Sensors which measure rotation are collectively known as `Gyroscopes' and there exist many technologies for their implementation, which are explained in \cite{gtfsa}. This project shall make use of MEMs gyroscopes because of their cost, which is orders of magnitude less than the alternative technologies.

\subsubsection{Gyroscopes}

MEMs gyroscopes are produced by many well known chip manufacturers, such as Freescale, ST, and Analog Devices. The gyroscopes that have been selected for use in the project are Analog Devices ADXRS614 \cite{adxrs}. These devices were chosen because they had a number of advantages over competitive devices:
\begin{itemize}
  \item A full-scale rate of $\pm$50$^\circ$/s, lowest in the field and so providing better resolution from the analogue output.
  \item Better vibration rejection above the sensor bandwidth
  \item More complete documentation
\end{itemize}

The disadvantage of MEMS accelerometers lies in the problem of integrating the angular rate in order to ascertain the pointing vector. The sensors have an analogue output, typically 0-5V, for which the angular rate is assumed linearly mapped from, in this case, -50 $\rightarrow$ +50$^\circ$/s. The `null' (zero angular rate) is nominally 2.5V but this varies as a function of manufacturing tolerance and environmental conditions such as temperature. For these gyroscopes the quoted figure is $\sim$0.3V for two standard deviations of 1000 samples \cite{adxrsds}. This is quite significant and a naive integration algorithm will lead to drift. Figure \ref{rawgyro} shows the result of such an algorithm, versus the true angle. This data was logged by rotating the inertial measurement unit with a servo sinusoidally with an amplitude of 5 degrees.  

\begin{figure}[!htbp]
  \centering
  \includegraphics[width=0.6\textwidth]{estimation/rawpics/rawgyro.eps}
  \caption{Graph demonstrating the effect of rate gyro bias on integration to obtain angle}
  \label{rawgyro}
\end{figure}

\subsubsection{Accelerometers}

The accelerometers were again of MEMS architecture. The are used to detect tilt, where a 90$^\circ$ deflection from horizontal in one direction measures 1G acceleration, and a 90$^circ$ deflection in the opposite direction measures -1G acceleration. The devices chosen are the ADXL203 from Analog Devices \cite{accelds}.

\subsubsection{Magnetometer} % (fold)
\label{ssub:Magnetometer}

The magnetometer is used to provide a reference to track yaw rate gyroscope drift. The model selected is the HMC5843 from Honeywell Devices \cite{magsens}. The Earth's magnetic field can be defined parametrically and the accepted model is the International Geomagnetic Reference Field \cite{igrf}.  

% subsubsection Magnetometer (end)

\subsection{Kalman Filter}

Figure \ref{rawgyro} demonstrates the problem of a naive integration of the rate gyroscope output to obtain an angular position. With knowledge of the sensor characteristics, such is process noise, and the system dynamics, a Kalman filter was designed to estimate the angle of the gyroscope by tracking the gyroscope bias in the state vector. The reference used for help in understanding the Kalman Filter is \cite{dsimon} and the notion of using the Kalman Filter to track gyroscope bias is from \cite{jmm}. The algorithm designed is as follows (described here for a single axis):

The raw sensors are measured and the values put into $y$,

\begin{eqnarray}
  K &=& PH'(HPH^T + VRV^T)^{-1} \\
  P &=& (I - KH)P \\
  x &=& x + K(y-h(x))
\end{eqnarray}

$x$ is recorded and then updated according to:

\begin{eqnarray}
  P &=& FPF^T + WQW^T \\
  x &=& f(x,y)
\end{eqnarray}

where the terms are defined as follows: 

\[
\begin{array}{ccc}
  x_k =
  \begin{bmatrix}
	\theta_k \\ \dot\theta_k \\ \text{bias}_{k}^{{gyro}}
  \end{bmatrix},
  &
  y_k =
  \begin{bmatrix}
	y_{k}^{gyro} \\ y_{k}^{accel}
  \end{bmatrix},
  &
  f(x_k,u_k) =
  \begin{bmatrix}
	{\theta_k + \delta(y_{k}^{gyro} - \text{bias}_{k}^{gyro}) } \\ {y_{k}^{gyro} - \text{bias}_{k}^{gyro}} \\ { \text{bias}_{k}^{gyro} }
  \end{bmatrix}

\end{array}
\]

\[
\begin{array}{cccc}
  F =
  \begin{bmatrix}
	1 & 0 & -\delta \\
	0 & 0 & -1 \\
	0 & 0 & 1 \\
  \end{bmatrix},
  &
  h(x_k)=
  \begin{bmatrix}
	\dot\theta_k + \text{bias}_{k}^{gyro} \\
	\theta_k \\
  \end{bmatrix},
  &
  H =
  \begin{bmatrix}
	0 & 1 & 1 \\
	1 & 0 & 0 \\
  \end{bmatrix},
  &
  I =
  \begin{bmatrix}
	1 & 0 & 0 \\
	0 & 1 & 0 \\
	0 & 0 & 1 \\
  \end{bmatrix}
\end{array}
\]

\[
\begin{array}{cccc}
  W = I, &
  V =
  \begin{bmatrix}
	1 & 0 \\
	0 & 1 \\
  \end{bmatrix},
  &
  R =
  \begin{bmatrix}
	\sigma^{2}_{gyro} & 0 \\
	0 & \sigma^{2}_{accel} \\
  \end{bmatrix},
  Q =
  \begin{bmatrix}
	0.15\delta \\ 0.15 \\ 0.1 \\ 
  \end{bmatrix}
  \begin{bmatrix}
	0.15\delta \\ 0.15 \\ 0.1 \\ 
  \end{bmatrix}^T

\end{array}
\]

The covariance matrix Q has values tuned by hand in this case to suit the specific hardware. When this algorithm is implemented and run is the same experimental setup as in Figure \ref{rawgyro}, the results can be seen in Figure \ref{kalmangyro}.

\begin{figure}[ht]
  \centering
  \includegraphics[width=0.8\textwidth]{estimation/rawpics/kalmangyro.eps}
  \caption{Graph demonstrating the use of a Kalman filter to estimate true angle by tracking gyroscope bias with an accelerometer}
  \label{kalmangyro}
\end{figure}

The results of the filter are very pleasing - the Kalman estimate tracks the true value very closely, and remained stable for extended periods.

